package ia;

import java.util.ArrayList;
import java.util.HashMap;

public class Astar {
	public static ArrayList<Checkpoint> getPath(NodeMap mm, Checkpoint cp1, Checkpoint cp2) {
		ArrayList<Checkpoint> closedset = new ArrayList<Checkpoint>();
		ArrayList<Checkpoint> openset = new ArrayList<Checkpoint>();
		openset.add(cp1);
		HashMap<Checkpoint, Checkpoint> came_from = new HashMap<Checkpoint, Checkpoint>();
		HashMap<Checkpoint, Float> g_score = new HashMap<Checkpoint, Float>();
		HashMap<Checkpoint, Float> f_score = new HashMap<Checkpoint, Float>();
		
		g_score.put(cp1, 0f);
		f_score.put(cp1, heuristic(cp1, cp2));
		
		while(!openset.isEmpty()) {
			float tmp_best_f = Float.MAX_VALUE;
			Checkpoint currentCp = null;

			// Checkpoint qui a le meilleur (plus petit) f
			for(Checkpoint cp : openset) {
				if(f_score.get(cp) < tmp_best_f) {
					tmp_best_f = f_score.get(cp);
					currentCp = cp;
				}
			}
			
			if(currentCp == cp2) {
				return rebuildPath(came_from, cp2);
			}
			
			openset.remove(currentCp);
			closedset.add(currentCp);
			
			for(Link link : currentCp.links()) {
				Checkpoint neighbour = link.next(currentCp);
				
				if(closedset.contains(neighbour))
					continue;
				
				float tmp_g_score = g_score.get(currentCp) + link.getDistance();
				boolean better = false;
				
				if(!openset.contains(neighbour)) {
					openset.add(neighbour);
					better = true;
				}
				else if(tmp_g_score < g_score.get(neighbour))
					better = true;
				
				if(better) {
					came_from.put(neighbour, currentCp);
					
					g_score.put(neighbour, tmp_g_score);
					f_score.put(neighbour, tmp_g_score + heuristic(neighbour,cp2));
					
					// Update(closedset, y);
					// Update(openset, y);
				}
			}
		}
		
		
		return null;
	}
	
	
	private static ArrayList<Checkpoint> rebuildPath(HashMap<Checkpoint, Checkpoint> came_from, Checkpoint currentCp) {
		if(came_from.containsKey(currentCp)) {
			ArrayList<Checkpoint> list = rebuildPath(came_from, came_from.get(currentCp));
			list.add(currentCp);
			
			return list;
		}
		else {
			ArrayList<Checkpoint> list = new ArrayList<Checkpoint>();
			list.add(currentCp);
			return list;
		}
	}
	
	
	private static float heuristic(Checkpoint cp1, Checkpoint cp2) {
		return cp1.coord.distance(cp2.coord);
	}
}
